EKF2: support multiple AGP sources by haumarco · Pull Request #26306 · PX4/PX4-Autopilot

haumarco · 2026-01-20T16:40:10Z

This PR is part of the split up of #26151

Solved Problem

EKF2 could only fuse a single Auxiliary Global Position (AGP) source, limiting the ability to use multiple external positioning systems simultaneously, e.g., visual navigation, pseudolites.

Solution

This PR enables EKF2 to fuse up to 4 independent AGP sources simultaneously:

New uORB message type (AuxGlobalPosition)
- Dedicated message separating AGP from VehicleGlobalPosition
- Includes id field to identify the source
- Includes source field for position source type (GNSS, Vision, Pseudolites, etc.)
- Versioned message for stable external API
Per-instance EKF2 parameters (EKF2_AGP0-3_*)
- EKF2_AGP{N}_ID: Sensor ID mapping (parameter slot → message ID)
- and therefore also: EKF2_AGP{N}_* CTRL, MODE, DELAY, NOISE, GATE
Refactored fusion architecture
- Manager + per-source AgpSource class pattern
- Each AGP source has independent state tracking, buffers, and fusion logic
- Dynamic parameter lookup without boilerplate switch statements
- Automatic sensor-to-slot mapping: message id field routes to parameter slot
- Per-source innovation monitoring and fault detection

Changelog Entry

For release notes:

Feature: Multi-instance Auxiliary Global Position fusion in EKF2
New parameters: EKF2_AGP{0-3}_{ID,CTRL,MODE,DELAY,NOISE,GATE}
New uORB message: AuxGlobalPosition (replaces aux_global_position topic usage of VehicleGlobalPosition)

Test coverage

Tested in simulation with mock mavlink and ROS messages. Tested in flight.

Known Limitations

Non-standard EKF pattern: The AGP code uses #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) because it places uORB subscriptions and parameter handling directly in the EKF library. The standard pattern (GNSS, EV) keeps uORB/params in EKF2.cpp and passes data via setXxxData() methods. This deviation enables dynamic multi-instance parameter lookup and self-contained source management, but is noted as technical debt.

Additional Notes

Extension of multi-sensor capability to Optical Flow or GNSS is planned. The manager + per-source pattern can be generalized to avoid duplicate code.

Depends on: DDS multi-instance PR #26305

github-actions · 2026-01-20T16:50:32Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 1056 byte (0.05 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1% +1.03Ki  +0.1% +1.03Ki    .text
  [NEW]    +976  [NEW]    +976    AgpSource::update()
  +0.3%    +468  +0.3%    +468    [section .text]
  [NEW]    +404  [NEW]    +404    ucdr_deserialize_sensor_optical_flow()
  [NEW]    +316  [NEW]    +316    ucdr_deserialize_aux_global_position()
  [NEW]    +256  [NEW]    +256    AgpSource::bufferData()
  [NEW]    +228  [NEW]    +228    AgpSource::AgpSource()
  [NEW]    +184  [NEW]    +184    AgpSource::isResetAllowed()
  [NEW]    +172  [NEW]    +172    AgpSource::initParams()
  +0.1%    +168  +0.1%    +168    g_cromfs_image
  +1.0%    +152  +1.0%    +152    px4::parameters
  [NEW]    +116  [NEW]    +116    AuxGlobalPosition::mapSensorIdToSlot()
  [NEW]     +84  [NEW]     +84    AuxGlobalPosition::getAgpParamInt32()
  [NEW]     +80  [NEW]     +80    AgpSource::updateParams()
 -99.8%     +72 -99.8%     +72    [18 Others]
 -19.4%     -76 -19.4%     -76    Ekf::updateParameters()
  -5.9%     -92  -5.9%     -92    EKF2::~EKF2()
 -31.5%    -112 -31.5%    -112    AuxGlobalPosition::AuxGlobalPosition()
  [DEL]    -184  [DEL]    -184    AuxGlobalPosition::isResetAllowed()
 -55.2%    -192 -55.2%    -192    Ekf::~Ekf()
  [DEL]    -744  [DEL]    -744    ucdr_deserialize_telemetry_status()
 -86.6% -1.19Ki -86.6% -1.19Ki    AuxGlobalPosition::update()
+0.0%    +899  [ = ]       0    .debug_abbrev
+0.1%    +152  [ = ]       0    .debug_aranges
+0.1%    +520  [ = ]       0    .debug_frame
+0.1% +37.8Ki  [ = ]       0    .debug_info
+0.0% +1.32Ki  [ = ]       0    .debug_line
   +67%      +2  [ = ]       0    [Unmapped]
  +0.0% +1.31Ki  [ = ]       0    [section .debug_line]
-0.0% -1.24Ki  [ = ]       0    .debug_loclists
-0.0%    -285  [ = ]       0    .debug_rnglists
  [DEL]      -1  [ = ]       0    [Unmapped]
  -0.0%    -284  [ = ]       0    [section .debug_rnglists]
-0.1% -2.54Ki  [ = ]       0    .debug_str
+1.3%      +3  [ = ]       0    .shstrtab
+0.1%    +381  [ = ]       0    .strtab
  [NEW]     +36  [ = ]       0    AgpSource::AgpSource()
  [NEW]     +76  [ = ]       0    AgpSource::bufferData()
  [NEW]     +28  [ = ]       0    AgpSource::initParams()
  [NEW]     +38  [ = ]       0    AgpSource::isResetAllowed()
  [NEW]     +51  [ = ]       0    AgpSource::update()
  [NEW]     +30  [ = ]       0    AgpSource::updateParams()
  [NEW]     +47  [ = ]       0    AuxGlobalPosition::getAgpParamInt32()
  [DEL]     -47  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  [NEW]     +44  [ = ]       0    AuxGlobalPosition::mapSensorIdToSlot()
   +12%      +7  [ = ]       0    AuxGlobalPosition::update()
  [DEL]     -43  [ = ]       0    AuxGlobalPosition::updateParamsImpl()
  +0.1%     +24  [ = ]       0    [section .strtab]
   +59%     +16  [ = ]       0    __arm_switchcontext_veneer
 -31.4%     -16  [ = ]       0    __nxsched_suspend_scheduler_veneer
  +0.1%      +4  [ = ]       0    do_not_explicitly_use_this_namespace::Param<>::update()
  [NEW]     +80  [ = ]       0    ucdr_deserialize_aux_global_position()
  [NEW]     +80  [ = ]       0    ucdr_deserialize_sensor_optical_flow()
  [DEL]     -74  [ = ]       0    ucdr_deserialize_telemetry_status()
+0.1%    +400  [ = ]       0    .symtab
  [NEW]     +64  [ = ]       0    AgpSource::AgpSource()
  [NEW]     +64  [ = ]       0    AgpSource::bufferData()
  [NEW]     +48  [ = ]       0    AgpSource::initParams()
  [NEW]     +16  [ = ]       0    AgpSource::isResetAllowed()
  [NEW]     +64  [ = ]       0    AgpSource::update()
  [NEW]     +16  [ = ]       0    AgpSource::updateParams()
  [NEW]     +64  [ = ]       0    AuxGlobalPosition::getAgpParamInt32()
  [DEL]     -16  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  [NEW]     +48  [ = ]       0    AuxGlobalPosition::mapSensorIdToSlot()
 -50.0%     -32  [ = ]       0    AuxGlobalPosition::update()
  [DEL]     -32  [ = ]       0    AuxGlobalPosition::updateParamsImpl()
 -14.3%     -16  [ = ]       0    AuxGlobalPosition::~AuxGlobalPosition()
   +11%     +16  [ = ]       0    EKF2::~EKF2()
   +33%     +16  [ = ]       0    Ekf::initialiseCovariance()
 -25.0%     -16  [ = ]       0    Ekf::reset()
 -50.0%     -16  [ = ]       0    FieldSensorBiasEstimator::updateEstimate()
 -25.0%     -32  [ = ]       0    RcvTopicsPubs::init()
  +1.1%    +128  [ = ]       0    [section .symtab]
   +67%     +32  [ = ]       0    __arm_switchcontext_veneer
 -25.0%     -16  [ = ]       0    __arp_arpin_veneer
-10.4% -1.03Ki  [ = ]       0    [Unmapped]
+0.1% +37.3Ki  +0.1% +1.03Ki    TOTAL

px4_fmu-v6x [Total VM Diff: 1032 byte (0.05 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1% +1.01Ki  +0.1% +1.01Ki    .text
  [NEW]    +976  [NEW]    +976    AgpSource::update()
  +0.3%    +480  +0.3%    +480    [section .text]
  [NEW]    +404  [NEW]    +404    ucdr_deserialize_sensor_optical_flow()
  [NEW]    +316  [NEW]    +316    ucdr_deserialize_aux_global_position()
  [NEW]    +256  [NEW]    +256    AgpSource::bufferData()
  [NEW]    +228  [NEW]    +228    AgpSource::AgpSource()
  [NEW]    +184  [NEW]    +184    AgpSource::isResetAllowed()
  [NEW]    +172  [NEW]    +172    AgpSource::initParams()
  +1.0%    +152  +1.0%    +152    px4::parameters
  +0.1%    +144  +0.1%    +144    g_cromfs_image
  [NEW]    +116  [NEW]    +116    AuxGlobalPosition::mapSensorIdToSlot()
  [NEW]     +84  [NEW]     +84    AuxGlobalPosition::getAgpParamInt32()
  [NEW]     +80  [NEW]     +80    AgpSource::updateParams()
 -99.8%     +60 -99.8%     +60    [19 Others]
 -19.4%     -76 -19.4%     -76    Ekf::updateParameters()
  -5.9%     -92  -5.9%     -92    EKF2::~EKF2()
 -31.5%    -112 -31.5%    -112    AuxGlobalPosition::AuxGlobalPosition()
  [DEL]    -184  [DEL]    -184    AuxGlobalPosition::isResetAllowed()
 -55.2%    -192 -55.2%    -192    Ekf::~Ekf()
  [DEL]    -744  [DEL]    -744    ucdr_deserialize_telemetry_status()
 -86.6% -1.19Ki -86.6% -1.19Ki    AuxGlobalPosition::update()
+0.0%    +899  [ = ]       0    .debug_abbrev
+0.1%    +152  [ = ]       0    .debug_aranges
+0.1%    +520  [ = ]       0    .debug_frame
+0.1% +36.6Ki  [ = ]       0    .debug_info
+0.0% +1.31Ki  [ = ]       0    .debug_line
 -16.7%      -1  [ = ]       0    [Unmapped]
  +0.0% +1.31Ki  [ = ]       0    [section .debug_line]
-0.0% -1.22Ki  [ = ]       0    .debug_loclists
-0.0%    -283  [ = ]       0    .debug_rnglists
   +50%      +1  [ = ]       0    [Unmapped]
  -0.0%    -284  [ = ]       0    [section .debug_rnglists]
-0.1% -2.50Ki  [ = ]       0    .debug_str
-0.4%      -1  [ = ]       0    .shstrtab
+0.1%    +377  [ = ]       0    .strtab
  [NEW]     +36  [ = ]       0    AgpSource::AgpSource()
  [NEW]     +76  [ = ]       0    AgpSource::bufferData()
  [NEW]     +28  [ = ]       0    AgpSource::initParams()
  [NEW]     +38  [ = ]       0    AgpSource::isResetAllowed()
  [NEW]     +51  [ = ]       0    AgpSource::update()
  [NEW]     +30  [ = ]       0    AgpSource::updateParams()
  [NEW]     +47  [ = ]       0    AuxGlobalPosition::getAgpParamInt32()
  [DEL]     -47  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  [NEW]     +44  [ = ]       0    AuxGlobalPosition::mapSensorIdToSlot()
   +12%      +7  [ = ]       0    AuxGlobalPosition::update()
  [DEL]     -43  [ = ]       0    AuxGlobalPosition::updateParamsImpl()
  +0.1%     +24  [ = ]       0    [section .strtab]
  [NEW]     +80  [ = ]       0    ucdr_deserialize_aux_global_position()
  [NEW]     +80  [ = ]       0    ucdr_deserialize_sensor_optical_flow()
  [DEL]     -74  [ = ]       0    ucdr_deserialize_telemetry_status()
+0.1%    +400  [ = ]       0    .symtab
  [NEW]     +64  [ = ]       0    AgpSource::AgpSource()
  [NEW]     +64  [ = ]       0    AgpSource::bufferData()
  [NEW]     +48  [ = ]       0    AgpSource::initParams()
  [NEW]     +16  [ = ]       0    AgpSource::isResetAllowed()
  [NEW]     +64  [ = ]       0    AgpSource::update()
  [NEW]     +16  [ = ]       0    AgpSource::updateParams()
  [NEW]     +64  [ = ]       0    AuxGlobalPosition::getAgpParamInt32()
  [DEL]     -16  [ = ]       0    AuxGlobalPosition::isResetAllowed()
  [NEW]     +48  [ = ]       0    AuxGlobalPosition::mapSensorIdToSlot()
 -50.0%     -32  [ = ]       0    AuxGlobalPosition::update()
  [DEL]     -32  [ = ]       0    AuxGlobalPosition::updateParamsImpl()
 -14.3%     -16  [ = ]       0    AuxGlobalPosition::~AuxGlobalPosition()
   +11%     +16  [ = ]       0    EKF2::~EKF2()
   +33%     +16  [ = ]       0    Ekf::initialiseCovariance()
 -25.0%     -16  [ = ]       0    Ekf::reset()
 -50.0%     -16  [ = ]       0    FieldSensorBiasEstimator::updateEstimate()
 -25.0%     -32  [ = ]       0    RcvTopicsPubs::init()
 -88.9%     +16  [ = ]       0    [5 Others]
  +1.1%    +128  [ = ]       0    [section .symtab]
   +50%     +16  [ = ]       0    __orb_aux_global_position
 -33.3%     -16  [ = ]       0    estimator::State::quat_nominal
 +68% +2.99Ki  [ = ]       0    [Unmapped]
+0.1% +40.2Ki  +0.1% +1.01Ki    TOTAL

Updated: 2026-02-20T16:12:58

sfuhrer

Have a look at the CI failure

bresch

Looks good to me, some minor comments

Add dedicated AuxGlobalPosition uORB message to replace the previous approach of reusing VehicleGlobalPosition for auxiliary global position sources.

Add per-instance parameters for auxiliary global position sources, allowing configuration of up to 4 AGP slots with individual source IDs.

Refactor auxiliary global position fusion to support multiple AGP sources. Add AgpSourceControl manager class that routes AGP messages to the correct AgpSource instance based on configured source IDs. Only instantiate AgpSource slots that have a configured ID on boot. Move AGP subscriptions to manager class for correct message routing.

Update integrations to use the new AuxGlobalPosition message instead of the VehicleGlobalPosition-based aux_global_position topic.

Update the AGP simulator to publish the new AuxGlobalPosition message type with the required source ID field.

haumarco requested a review from bresch January 20, 2026 16:40

This was referenced Jan 20, 2026

Mavlink: GLOBAL_POSITION for aux positioning #26307

Merged

EKF2: Multi-instance AGP fusion #26151

Closed

beniaminopozzan mentioned this pull request Jan 20, 2026

uxrce_dds: support multi-instance uORB topics #26305

Merged

haumarco force-pushed the agp-multi-source branch 2 times, most recently from 3d71568 to fb42fb5 Compare January 23, 2026 11:15

bresch requested changes Jan 23, 2026

View reviewed changes

haumarco force-pushed the agp-multi-source branch from fb42fb5 to adedf00 Compare January 23, 2026 12:02

bresch requested changes Jan 23, 2026

View reviewed changes

Comment thread src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

sfuhrer reviewed Jan 29, 2026

View reviewed changes

hamishwillee reviewed Feb 3, 2026

View reviewed changes

Comment thread msg/versioned/AuxGlobalPosition.msg Outdated

haumarco force-pushed the agp-multi-source branch from 81c1c6c to a016128 Compare February 12, 2026 17:08

bresch requested changes Feb 19, 2026

View reviewed changes

Comment thread src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp Outdated

Comment thread src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp Outdated

haumarco added 5 commits February 19, 2026 17:51

uorb: add AuxGlobalPosition message

a1f0e1c

Add dedicated AuxGlobalPosition uORB message to replace the previous approach of reusing VehicleGlobalPosition for auxiliary global position sources.

ekf2: add multi-instance AGP parameters

922688a

Add per-instance parameters for auxiliary global position sources, allowing configuration of up to 4 AGP slots with individual source IDs.

ekf2: update logger, mavlink, DDS, and replay for AuxGlobalPosition

347cdc1

Update integrations to use the new AuxGlobalPosition message instead of the VehicleGlobalPosition-based aux_global_position topic.

simulation: update sensor_agp_sim for AuxGlobalPosition message

be0d0f3

Update the AGP simulator to publish the new AuxGlobalPosition message type with the required source ID field.

haumarco force-pushed the agp-multi-source branch from 4cd9c6b to 0d115d7 Compare February 19, 2026 16:58

haumarco mentioned this pull request Feb 20, 2026

Switch aux_global_position to AuxGlobalPosition message type Auterion/px4-ros2-interface-lib#180

Merged

add translation for aux_global_position

2324aef

haumarco force-pushed the agp-multi-source branch from 0d115d7 to 2324aef Compare February 20, 2026 15:23

fix ros-test-config for multi-APG

e03eaf4

bresch approved these changes Feb 20, 2026

View reviewed changes

haumarco merged commit 7297364 into main Feb 20, 2026
75 checks passed

haumarco deleted the agp-multi-source branch February 20, 2026 16:43

mrpollo mentioned this pull request Feb 21, 2026

msg_docs: fix IndexError crash in generate_msg_docs.py #26546

Merged

PetervdPerk-NXP mentioned this pull request Feb 25, 2026

zenoh: fix default config topic type to use actual uORB topic name #26564

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

EKF2: support multiple AGP sources#26306

EKF2: support multiple AGP sources#26306
haumarco merged 7 commits intomainfrom
agp-multi-source

haumarco commented Jan 20, 2026

Uh oh!

github-actions bot commented Jan 20, 2026 •

edited

Loading

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

sfuhrer left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bresch left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

Conversation

haumarco commented Jan 20, 2026

Solved Problem

Solution

Changelog Entry

Test coverage

Known Limitations

Additional Notes

Uh oh!

github-actions bot commented Jan 20, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🔎 FLASH Analysis

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

sfuhrer left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bresch left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

github-actions bot commented Jan 20, 2026 •

edited

Loading